#include "Ai.h"
#include <cmath>
#include <windows.h>

int Ai::fieldSize;
int Ai::mapXmulFieldSize;
int Ai::mapYmulFieldSize;
int Ai::mapZmulFieldSize;

Ai::Ai()
{
	map = NULL;	
}

Ai::~Ai()
{
	Destroy();
}

void Ai::Destroy(void)
{	
	delete[] map;
	map = NULL;
}

list<Boid*>* Ai::MapField(float x, float y, float z)
{
	int X = ((int)x + border) / fieldSize;
	int Y = (int)y / fieldSize;
	int Z = ((int)z + border) / fieldSize;
	return &map[X + mapX * ( Y + mapY * Z)];
}

bool Ai::IsInsideScene(float x, float y, float z)
{
	if((int)x + border < 0)
		return false;
	if(y < 0.0f)
		return false;
	if((int)z + border < 0)
		return false;
	if((int)x + border >= mapXmulFieldSize)
		return false;
	if((int)y + border >= mapYmulFieldSize)
		return false;
	if((int)z + border >= mapZmulFieldSize)
		return false;
	return true;
}

void Ai::Init(Physics* scene, Map *map, int border)
{
	scene->SetCollisionHandler(&boidCollisionHandler);
	Boid::SetViewDistanceAllDirections(1.0f);
	Boid::SetViewDistanceForward(2.5f);
	Boid::SetMinDistance(0.5f);
	Boid::SetMaxSpeed(1.0f);
	Boid::SetRedStartEnergy(10.0f);
	Boid::SetYellowEnergy(3.0f);
	srand(GetTickCount());

	fieldSize = (int)(ceilf(2.0f * Boid::ViewDistanceAllDirections()));
	this->border = border;
	mapX = (map->Columns() + 2 * border + fieldSize - 1) / fieldSize;
	mapY = ((int)ceilf(map->GetHighest()) + border + fieldSize - 1) / fieldSize;
	mapZ = (map->Rows() + 2 * border + fieldSize - 1) / fieldSize;

	mapXmulFieldSize = mapX * fieldSize;
	mapYmulFieldSize = mapY * fieldSize;
	mapZmulFieldSize = mapZ * fieldSize;

	this->map = new list<Boid*>[mapX * mapY * mapZ];
}

void Ai::Maintain(Physics *scene, float time)
{	
	list<Boid*>::iterator boid;
	Vector3 position;
	
	//Update fields
	for(boid = boids.begin(); boid != boids.end(); ++boid)
	{
		position = (*boid)->LastPosition();
		list<Boid*>* lastField = MapField(position.x, position.y, position.z);
		position = (*boid)->Position();
		list<Boid*>* currentField = MapField(position.x, position.y, position.z);		
		if(currentField != lastField)
		{
			lastField->remove(*boid);
			currentField->push_back(*boid);			
		}
		(*boid)->SetLastPosition(position);
	}

	//Maintain
	for(boid = boids.begin(); boid != boids.end(); ++boid)
	{		
		MaintainBoid(*boid, time, scene);
	}
}

void Ai::MaintainBoid(Boid* boid, float time, Physics *scene)
{
	//Find visible boids
	Vector3 position;
	Vector3 p;
	list<list<Boid*>*> boidLists;
	static list<Boid*>* usedFields[16];
	list<Boid*>* field;
	int usedFieldsCount = 0;
	int i, j, x, y, z;
	position = boid->Position();
	//find fields visible in all directions	
	for(p.x = position.x - Boid::ViewDistanceAllDirections(), x = -1; x < 2; x++, p.x += Boid::ViewDistanceAllDirections())
	{
		for(p.y = position.y - Boid::ViewDistanceAllDirections(), y = -1; y < 2; y++, p.y += Boid::ViewDistanceAllDirections())
		{
			for(p.z = position.z - Boid::ViewDistanceAllDirections(), z = -1; z < 2; z++, p.z += Boid::ViewDistanceAllDirections())
			{								
				if(IsInsideScene(p.x, p.y, p.z))
				{
					field = MapField(p.x, p.y, p.z);
					for(i = 0; i < usedFieldsCount; i++)
					{
						if(field == usedFields[i])
							break;
					}
					if(i == usedFieldsCount)
					{
						usedFields[usedFieldsCount] = field;
						usedFieldsCount++;
						boidLists.push_back(field);
					}
				}
			}
		}
	}
	
	Vector3 collision(0, 0, 0);

	//find fields visible froward
	Vector3 velocity = boid->Velocity();
	if(velocity.Length() > 0.000001f)
	{
		Vector3 velocityNormalized = velocity;
		velocityNormalized.Normalize();
		static float smallViewDistanceForward = Boid::ViewDistanceForward() / 8.0f;
		Vector3 pDiff = velocityNormalized * smallViewDistanceForward;
		p = position;
		for(int i = 1; i <= 8; i++)
		{
			p += pDiff;
			if(IsInsideScene(p.x, p.y, p.z))
			{
				field = MapField(p.x, p.y, p.z);
				for(j = 0; j < usedFieldsCount; j++)
				{
					if(field == usedFields[j])
						break;
				}
				if(j == usedFieldsCount)
				{
					usedFields[usedFieldsCount] = field;
					usedFieldsCount++;
					boidLists.push_back(field);
				}
			}
		}

		if(boid->Type() == Boid::BoidType::Yellow)
			collision = AvoidCollision(boid, scene);				
	}
	list<Boid*> boids;
	FindVisibleBoids(boid, &boidLists, &boids);
	if(boid->Type() == Boid::BoidType::Yellow)
		MaintainYellow(boid, &boids, collision, time);
	else
		MaintainRed(boid, &boids, collision, time);
}

Vector3 Ai::AvoidCollision(Boid *boid, Physics *scene)
{
	Vector3 velocity = boid->Velocity();
	Vector3 velocityNormalized = velocity;
	Vector3 collision(0, 0, 0);
	Vector3 position = boid->Position();
	velocityNormalized.Normalize();

	Vector3 moves[4];
	moves[0] = velocityNormalized * Boid::ViewDistanceForward();
	moves[1] = moves[2] = moves[3] = moves[0];
	Vector3 up(0, 1, 0);
	Vector3 side;		
	if(scene->IsCollidingSetOfBoxes(position, position + moves[0], 1.2f * DynamicSphere::Radius()))
	{
		side = Vector3::CrossProduct(up, velocityNormalized);
		if(side.Length() > 0.000001f)
		{
			side.Normalize();					
			int i, j, k, r;
			for(i = 0; i < 4; i++)
			{
				moves[0] += side;
				moves[1] -= side;
				moves[2] += up;
				moves[3] -= up;
				r = rand();
				for(j = 0; j < 4; j++)
				{
					k = (j + r) & 3;
					if(!scene->IsCollidingSetOfBoxes(position, position + moves[k], 1.2f * DynamicSphere::Radius()))
					{
						up = Vector3::CrossProduct(velocityNormalized, moves[k]);
						side = Vector3::CrossProduct(up, velocityNormalized);						
						collision = side;	
						if(collision.Length() >0.000001f)
							collision.Normalize();
						break;
					}
				}
				if(j < 4)
					break;
			}
			if(i == 4)	//Change direction any way
				collision = (moves[rand() & 3].Normalize());
		}
	}
	return collision;
}

void Ai::FindVisibleBoids(Boid *boid, list<list<Boid*>*>* boidLists, list<Boid*>* visibleBoids)
{
	list<list<Boid*>*>::iterator boids;
	list<Boid*>::iterator currentBoid;
	Vector3 position = boid->Position();
	Vector3 secondPosition;
	Vector3 v;
	Vector3 velocityNormalized = boid->Velocity();
	bool isVelocityZero = false;
	float distance;
	if(velocityNormalized.Length() < 0.000001f)	
		isVelocityZero = true;	
	else
		velocityNormalized.Normalize();

	for(boids = boidLists->begin(); boids != boidLists->end(); ++boids)
	{
		for(currentBoid = (*boids)->begin(); currentBoid != (*boids)->end(); ++currentBoid)
		{
			if(boid == *currentBoid)
				continue;
			secondPosition = (*currentBoid)->Position();
			v = secondPosition - position;
			distance = v.Length();
			if(distance < Boid::ViewDistanceAllDirections())
			{
				visibleBoids->push_back(*currentBoid);
				continue;
			}
			if(isVelocityZero)
				continue;
			if(distance > Boid::ViewDistanceForward())
				continue;
			v.Normalize();
			if(Vector3::DotProduct(velocityNormalized, v) > 0.86f)	//30 degree
				visibleBoids->push_back(*currentBoid);
		}
	}
}

void Ai::MaintainYellow(Boid *boid, list<Boid*>* boids, Vector3 &collision, float time)
{
	//iterate all boids
	Vector3 position = boid->Position();
	Vector3 secondPosition;
	Vector3 v(0, 0, 0);
	Vector3 separation(0, 0, 0);
	Vector3 run(0, 0, 0);
	Vector3 velocity = boid->Velocity();
	Vector3 center(0, 0, 0);
	Vector3 commonVelocity(0, 0, 0);
	int count = 0;
	float distance;		
	list<Boid*>::iterator currentBoid;
	for(currentBoid = boids->begin(); currentBoid != boids->end(); ++currentBoid)
	{
		secondPosition = (*currentBoid)->Position(); 
		v = position - (*currentBoid)->Position();
		if((*currentBoid)->Type() == Boid::BoidType::Red)
		{			
			
			if(v.Length() > 0.000001)
				v.Normalize();				
			run += v;
		}
		else
		{							
			distance = v.Length();			
			center += secondPosition;
			commonVelocity += (*currentBoid)->Velocity();
			count++;																
			if(distance < Boid::MinDistance())
			{
				if(distance > 0.000001f)
				{
					v.Normalize();
					separation += v;
				}			
			}
		}
	}

	v = Vector3(0, 0, 0);
	if(run.Length() > 0.000001f)
	{
		run.Normalize();
		run *= 3.0f;
		v += run;
	}
	if(separation.Length() > 0.000001f)
	{
		separation.Normalize();
		v += separation;
	}
	if(count > 0)
	{
		commonVelocity /= (float)count;
		center /= (float)count;	
		center -= boid->Position();
		if(center.Length() > 0.000001f)
		{
			center.Normalize();
			v += center;
		}
		if(commonVelocity.Length() > 0.000001f)
		{
			commonVelocity.Normalize();
			v += commonVelocity;
		}		
	}
	v += collision;
	v *= time;
	v += boid->Velocity();	
	if(v.Length() > Boid::MaxSpeed())
	{
		v.Normalize();
		v *= Boid::MaxSpeed();
	}	
	boid->SetVelocity(v);
}

void Ai::MaintainRed(Boid *boid, list<Boid*>* boids, Vector3 &collision, float time)
{	
	if(boid->Energy() <= 0.0f)
	{
		boid->SetType(Boid::BoidType::Yellow);
		return;
	}
	Boid *bestPrey = NULL;
	float minCost = 1000000.0f;
	list<Boid*>::iterator currentBoid;
	Vector3 position = boid->Position();
	Vector3 velocity = boid->Velocity();
	Vector3 preyPosition;
	Vector3 preyVelocity;
	Vector3 v1, v2;
	float distance;	
	float cost;

	//choose best prey
	for(currentBoid = boids->begin(); currentBoid != boids->end(); ++currentBoid)
	{
		if((*currentBoid)->Type() == Boid::BoidType::Red)
			continue;
		preyPosition = (*currentBoid)->Position();
		preyVelocity = (*currentBoid)->Velocity();
		v1 = preyPosition - position;
		v2 = velocity + preyVelocity;
		distance = v1.Length();
		if(distance > 0.000001f)
		{
			v1.Normalize();
			if(v2.Length() > 0.000001f)
			{
				v2.Normalize();
				cost = distance * (1.5f - (Vector3::DotProduct(v1, v2)));
			}
			else
				cost = distance;
		}
		else
			return;
		if(cost < minCost)
		{
			minCost = cost;
			bestPrey = *currentBoid;
		}
	}
	if(bestPrey == NULL)
		return;	
	
	//chase prey
	v1 = bestPrey->Position() - position;
	v2 = velocity - bestPrey->Velocity();
	float speed = v2.Length();	
	distance = v1.Length();
	float approximatedTime = distance / speed;
	if(approximatedTime > 5.0f)
		approximatedTime = 5.0f;

	v2 = v1 - approximatedTime * v2;	
	v2 *= time * 2.0f * speed;
	boid->SetEnergy(boid->Energy() - v2.Length());
	boid->SetVelocity(velocity + v2);
}

void Ai::AddBoid(Boid* boid)
{
	boids.push_back(boid);	
	Vector3 position = boid->Position();
	boid->SetLastPosition(position);
	MapField(position.x, position.y, position.z)->push_back(boid);
}

void Ai::RemoveBoid(Boid* boid)
{
	boids.remove(boid);
	Vector3 position = boid->Position();
	MapField(position.x, position.y, position.z)->remove(boid);
}

list<DynamicSphere*>* Ai::EatenBoids(void)
{
	return boidCollisionHandler.EatenBoids();
}